perm filename IMOVE2.2[EAL,HE]3 blob sn#708945 filedate 1983-05-01 generic text, type C, neo UTF8
COMMENT ⊗   VALID 00009 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	{$NOMAIN	Small "move-related" statement interpreters }
C00028 00003	{ Externally defined routines: }
C00031 00004	procedure doCmon external
C00038 00005	procedure doOperate external
C00042 00006	procedure doOpen (* & doClose *) external
C00049 00007	procedure doCenter external
C00051 00008	procedure doStop external
C00053 00009	procedure doArmmagic external
C00057 ENDMK
C⊗;
{$NOMAIN	Small "move-related" statement interpreters }

const

	GARMDEV   = 1;			(* device numbers for ARM *)
	VISEDEV   = 6;

	SIGMAG = 20000B;	(* Test only magnitude of forces *)
	SIGGE = 100000B;	(* Start cmon if force ≥ specified value *)
	SIGLT = 0B;		(*   "	  "  "	  "   <	    "	    " *)

	FSTOP  = (*10000B*)4096;	(* In addition to starting cmon, stop arm *)

	nullingcb = 512;	(* control bits for trajectory specs *)
	dureqcb = 192;
	destptcb = 8;
	Byptcb = 256;		Linearcb = 256;		(* 400B *)

  (* Constants from EDIT *)

  maxLines = 28;
  maxPPLines = 18;
  maxBpts = 25;
  maxTBpts = 20;	(* max could be exceeded by huge case stmnt *)
  listinglength = 2000;	(* Length of Listingarray *)

(* Random type declarations for OMSI/SAIL compatibility *)

type
  byte = 0..255;	(* doesn't really belong here, but... *)
  ascii = char; 
  atext = text;

{ Define all the pointer types here }

vectorp = ↑vector;
transp = ↑trans;
strngp = ↑strng;
eventp = ↑event;
framep = ↑frame;
statementp = ↑statement;
varidefp = ↑varidef;
nodep = ↑node;
pdbp = ↑pdb;
envheaderp = ↑envheader;
enventryp = ↑enventry;
environp = ↑environment;
cmoncbp = ↑cmoncb;
messagep = ↑message;

(* This one is used whenever a pointer is needed for which the 	*)
(* definition is missing from this file; naturally, all 	*)
(* pointers use the same space 					*)

dump = ↑integer;
token = array[1..4] of integer;		{Uses same space as a token}
cursorp = array[1..4] of integer;	{Ditto, for cursorp}


(* datatype definitions *)

datatypes = (pconstype, varitype, svaltype, vectype, rottype, transtype,
	     frametype, eventtype, strngtype, labeltype, proctype, arraytype,
	     reftype, valtype, cmontype, nulltype, undeftype,
	     dimensiontype, mactype, macargtype, freevartype);

scalar = real;

u = (used,free);
vector = record case u of
	   used: (refcnt: integer; val: array [1..3] of real);
	   free: (next: vectorp);
	 end;

trans = record case u of
	   used: (refcnt: integer; val: array [1..3,1..4] of real);
	   free: (next: transp);
	end;

cstring = packed array [1..10] of ascii;
c4str = packed array [1..4] of ascii;
c5str = packed array [1..5] of ascii;
c20str = packed array [1..20] of ascii;
linestr = packed array [1..130] of ascii;

strng = record
	  next: strngp;
	  ch: cstring;
	end;

event = record
	  next: eventp;		(* all events are on one big list *)
	  count: integer;
	  waitlist: pdbp;
	end;

frame = record
	  vari: varidefp;	(* back pointer to variable name & info *)
	  calcs: nodep;		(* affixment info *)
	  case ftype: boolean of	(* frame = true, device = false *)
  true:	    (valid: integer; val, fdepr: transp; dcntr: integer; dev: framep);
  false:    (mech: integer; case sdev: boolean of
		true: (sdest: real); false: (tdest,appr,depr: transp));
		(* sdev = true for scalar devices, false for frames *)
	end;



(* statement definitions *)

stmntypes = (progtype, blocktype, coblocktype, endtype, coendtype,
		fortype, iftype, whiletype, untiltype, casetype,
		calltype, returntype,
		printtype, prompttype, pausetype, aborttype, assigntype,
		signaltype, waittype, enabletype, disabletype, cmtype,
		affixtype, unfixtype,
		movetype,jtmovetype,operatetype,opentype,closetype,centertype,
		floattype, stoptype, retrytype,
		requiretype, definetype, macrotype, commenttype, dimdeftype,
		setbasetype, wristtype, saytype, declaretype, emptytype,
		evaltype, armmagictype);
		(* more??? *)

statement = packed record
		next, last: statementp;
		stlab: varidefp;
		exprs: nodep;	(* any expressions used by this statement *)
		nlines: integer;
		bpt,bad: boolean;
		case stype: stmntypes of

    coendtype:	    (bcode, bparent: statementp; blkid: dump;
			level, numvars: 0..255; variables: varidefp);
    movetype,
    jtmovetype,
    operatetype,
    opentype,
    closetype,
    centertype,
    floattype,
    setbasetype,
    stoptype:	    (cf, clauses: nodep);
    armmagictype:   (cmdnum,dev,iargs,oargs: nodep);
    cmtype:	    (oncond: nodep; conclusion: statementp;
			deferCm, exprCm: boolean; cdef: varidefp);
		end;



(* auxiliary definitions: variable, etc. *)

varidef = packed record
	    next,dnext: varidefp;
	    name: dump;
	    level: 0..255;	(* environment level *)
	    offset: 0..255;	(* environment offset *)
	    dtype: varidefp;	(* to hold the dimension info *)
	    tbits: 0..15;  (* special type bits: array = 1, proc = 2, ref = 4 & ? *)
	    dbits: 0..15;	(* for use by debugger/interpreter *)
	    case vtype: datatypes of
  arraytype:  (a: nodep);
  proctype:   (p: nodep);
  labeltype,
  cmontype:   (s: statementp);
  mactype:    (mdef: statementp);
  macargtype: (marg: dump);
  pconstype:  (c: nodep);
  dimensiontype: (dim: nodep);
	  end;



(* definition of the ubiquitous NODE record *)

nodetypes = (exprnode, leafnode, listnode, clistnode, colistnode, forvalnode,
		deprnode, viaptnode, apprnode, destnode, byptnode, durnode,
		sfacnode, wobblenode, swtnode, nullingnode, wristnode, cwnode,
		arrivalnode, departingnode,
		ffnode, forcenode, stiffnode, gathernode, cmonnode, errornode,
		calcnode, arraydefnode, bnddefnode, bndvalnode,
		waitlistnode, procdefnode, tlistnode, dimnode, commentnode,
		linearnode, elbownode, shouldernode, flipnode, wrtnode,
		loadnode,velocitynode);

exprtypes =  (	svalop,					(* scalar operators *)
		sltop, sleop, seqop, sgeop, sgtop, sneop,	(* relations *)
		notop, orop, xorop, andop, eqvop,		(* logical *)
		saddop, ssubop, smulop, sdivop, snegop, sabsop, (* scalar ops *)
		sexpop, maxop, minop, intop, idivop, modop,
		sqrtop, logop, expop, timeop,			(* functions *)
		sinop, cosop, tanop, asinop, acosop, atan2op,	(* trig *)
		vdotop, vmagnop, tmagnop,
		vecop,					(* vector operators *)
		vmakeop, unitvop, vaddop, vsubop, crossvop, vnegop,
		svmulop, vsmulop, vsdivop, tvmulop, wrtop,
		tposop, taxisop,
		transop,				(* trans operators *)
		tmakeop, torientop, ttmulop, tvaddop, tvsubop, tinvrtop,
		vsaxwrop, constrop, ftofop, deproachop, fmakeop, vmkfrcop,
		ioop,					(* i/o operators *)
		queryop, inscalarop,
		specop,					(* special operators *)
		arefop, callop, grinchop, macroop, vmop, adcop, dacop, jointop,
		badop,
		addop, subop, negop, mulop, divop, absop); (* for parsing *)

leaftypes = pconstype..strngtype;

reltypes = sltop..sgtop;
forcetypes = (force,absforce,torque,abstorque,angvelocity);

node = record
	next: nodep;
	case ntype: nodetypes of
    exprnode:	(op: exprtypes; arg1, arg2, arg3: nodep; elength: integer);
    leafnode:	(case ltype: leaftypes of
	varitype:  (vari: varidefp; vid: dump);
	pconstype: (cname: varidefp; pcval: nodep);
	svaltype:  (s: scalar; wid: integer);
	vectype:   (v: vectorp);
	transtype: (t: transp);
	strngtype: (length: integer; str: strngp) ); (* also used by commentnodes *)
    arrivalnode:(evar: varidefp);
    wrtnode,
    deprnode,
    apprnode,
    destnode:	(loc: nodep; code: statementp);
    byptnode,
    viaptnode:	(vlist: boolean; via,vclauses: nodep; vcode: statementp);
    durnode:	(durrel: reltypes; durval: nodep);
    velocitynode,
    sfacnode,
    wobblenode,
    swtnode:	(clval: nodep);
    nullingnode,			(* true = nonulling *)
    wristnode,				(*	= don't zero force wrist *)
    cwnode,				(*	= counter_clockwise *)
    elbownode,				(*	= elbow up *)
    shouldernode,			(*	= right shoulder *)
    flipnode,				(*	= don't flip wrist *)
    linearnode:	(notp: boolean);	(*	= linear motion *)
    ffnode:	(ff,cf: nodep; csys, pdef: boolean); (* true = world, false = hand *)
    loadnode:	(loadval,loadvec: nodep; lcsys: boolean); (* lcsys = csys above *)
    forcenode:	(ftype: forcetypes; frel: reltypes; fval, fvec, fframe: nodep);
    stiffnode:	(fv, mv, cocff: nodep);
    gathernode:	(gbits: integer);
    cmonnode:	(cmon: statementp; errhandlerp: boolean);
    errornode:	(eexpr: nodep);
	end;



(* process descriptor blocks & environment record definitions *)

queuetypes = (nullqueue,nowrunning,runqueue,inputqueue,eventqueue,sleepqueue,
		forcewait,devicewait,joinwait,proccall);

pdb = packed record
	nextpdb,next: pdbp;	(* for list of all/active pdb's *)
	level: 0..255;		(* lexical level *)
	mode: 0..255;		(* expression/statement/sub-statement *)
	priority: 0..255;	(* probably never greater than 3? *)
	status: queuetypes;	(* what are we doing *)
	env: envheaderp;
	spc: statementp;	(* current statement *)
	epc: nodep;		(* current expression (if any) *)
	sp: nodep;		(* intermediate value stack *)
	cm: cmoncbp;		(* if we're a cmon point to our definition *)
	mech: framep;		(* current device being used *)
	linenum: integer;	(* used by editor/debugger *)
	 case procp: boolean of	(* true if we're a procedure *)
true:  (opdb: pdbp;		(* pdb to restore when procedure exits *)
	pdef: nodep);		(* procedure definition node *)
false: (evt: eventp;		(* event to signal when process goes away *)
	sdef: statementp);	(* first statement where process was defined *)
      end;


envheader = packed record
	      parent: envheaderp;
	      env: array [0..4] of environp;
	      varcnt: 0..255;		(* # of variables in use ??? *)
		case procp: boolean of  (* true if we're a procedure *)
	true: (proc: nodep);
	false:(block: statementp);
	    end;

enventry = record
	    case etype: datatypes of
  svaltype:  (s: scalar);
  vectype:   (v: vectorp);
  transtype: (t: transp);
  frametype: (f: framep);
  eventtype: (evt: eventp);
  strngtype: (length: integer; str: strngp);
  cmontype:  (c: cmoncbp);
  proctype:  (p: nodep; penv: envheaderp);
  reftype:   (r: enventryp);
  arraytype: (a: envheaderp; bnds: nodep);
	   end;


environment = record
		next: environp;
		vals: array [0..9] of enventryp;
	      end;


cmoncb = record
	   running, enabled: boolean;		(* cmon's status *)
	   cmon: statementp;
	   pdb: pdbp;
	   evt: eventp;
	   fbits: integer;			(* bits for force sensing *)
	   oldcmon: cmoncbp;			(* for debugger *)
	 end;



(* definition of AL-ARM messages *)

msgtypes = (initarmscmd,calibcmd,killarmscmd,wherecmd,
	    abortcmd,stopcmd,movehdrcmd,movesegcmd,
	    centercmd,operatecmd,movedonecmd,signalcmd,
	    setccmd,forcesigcmd,forceoffcmd,biasoncmd,biasoffcmd,setstiffcmd,
	    zerowristcmd,wristcmd,gathercmd,getgathercmd,readadccmd,writedaccmd,
	    errorcmd,floatcmd,setloadcmd,
	    armmagiccmd,realcmd,vectorcmd,transcmd);

errortypes = (noerror,noarmsol,timerr,durerr,toolong,featna,
	      unkmess,srvdead,adcdead,nozind,exjtfc,paslim,nopower,badpot,devbusy,
	      baddev,timout,panicb,nocart,cbound,badparm);

message = record
	   cmd: msgtypes;
	   ok: boolean;
	   case integer of
	1:   (dev, bits, n: integer;
(*	     (dev, bits, n, evt: integer;	(* for arm code version *)
	      evt: eventp;
	      dur: real;
	      case integer of
		1: (v1,v2,v3: real);
		2: (sfac,wobble,pos: real);
		3: (val,angle,mag: real);
		4: (max,min: real);
		5: (error: errortypes));
	2:   (fv1,fv2,fv3,mv1,mv2,mv3: real);	(* may never use these... *)
	3:   (t: array [1..6] of real);
	  end;

interr = record
         case integer of
           0: (i: integer);
	   1: (err,foo: errortypes);
	 end;

listingarray = packed array [0..listinglength] of ascii;


(* global variables *)

var
	(* from EDIT *)
    listing: listingarray;  (* first 150 chars are used by expression editor *)
			    (* next 40 by header & trailer lines *)
{*} cursorStack: array [1..15] of cursorp;	{These are BIG records! }
(*  lbuf: array [1..160] of ascii;
    ppBuf: array [1..100] of ascii; *)
    dum1: array[1..260] of ascii;
    lines: array [1..maxLines] of dump; 
    ppLines: array [1..maxPPLines] of dump;	
(*  marks: array [1..20] of integer;
    reswords: array [0..26] of reswordp;
    idents: array [0..26] of identp;
    macrostack: array [1..10] of tokenp;
    curmacstack: array [1..10] of varidefp;
    screenheight,dispHeight: integer;
    ppBufp,oppBufp,ppOffset,ppSize,nmarks: integer;
    lbufp,cursor,ocur,cursorLine,fieldnum,lineNum,findLine,pcLine: integer;
    firstDline,topDline,botDline,firstLine,lastLine,curLine: integer;
    freeLines,oldLines: linerecp;
    sysVars: varidefp;
    dProg: statementp;
    curBlock, newDeclarations, findStmnt: statementp;
    macrodepth: integer;
    filedepth, errCount, sCursor: integer;
    curChar, maxChar, curFLine, curPage: integer;
    nodim, distancedim, timedim, angledim,
      forcedim, torquedim, veldim, angveldim: varidefp;
    fvstiffdim, mvstiffdim: nodep;
    pnode: nodep;
*)  dum2: array[1..141] of dump;
(*  smartTerminal: boolean; 
    setUp,setExpr,setCursor,dontPrint,outFilep,collect,fParse,sParse,
      eofError,endOfLine,backup,expandmacros,flushcomments,checkDims,
      shownLine: boolean;
*)  dum3: array[1..16] of boolean;
    curtoken: token;
    file1,file2,file3,file4,file5,outFile: atext;

    bpts: array [1..maxBpts] of statementp;	(* debugging crap *)
    tbpts: array [1..maxTBpts] of statementp;
    debugPdbs: array [0..10] of pdbp;
(*  nbpts,ntbpts,debugLevel: integer;
    eCurInt: pdbp;
    STLevel: integer;
*)  dum4: array[1..5] of integer;
    singleThreadMode,tSingleThreadMode: boolean;

	(* from INTERP *)
    inputLine: array [1..20] of ascii;
    talk: text;			(* for using the speech synthesizer *)
    curInt, activeInts, readQueue, allPdbs: pdbp;
    sysEnv: envheaderp;
    clkQueue: nodep;
    allEvents: eventp;
    etime: integer;		(* used by eval *)
    curtime: integer; (* who knows where this will get updated - an ast? *)
    stime: integer;		(* used for clock queue on 10 *)
    msg: messagep;		(* for AL-ARM interaction *)
    inputp: integer;		(* current offset into inputLine array above *)
    resched, running, escapeI, iSingleThreadMode: boolean;
    msgp: boolean;		(* flag set if any messages pending *)
    inputReady: boolean;

(* various constant pointers *)
    xhat,yhat,zhat,nilvect: vectorp;
    niltrans: transp;
    gpark, rpark: transp;		(* arm park positions *)

(* various device & variable pointers *)
    speedfactor: enventryp;
    garm: framep;

{ Externally defined routines: }

	(* From ALLOC *)
procedure relNode(n: nodep);					external;

	(* From IAUX1A *)
function pop: nodep;						external;
function getEntry (level, offset: byte): enventryp; 		external;
function getVar (level, offset: byte): enventryp;		external;
function gtVarn (n: nodep): enventryp;				external;
function getNval(n: nodep; var b: boolean): nodep;		external;
function getEvent: eventp;					external;
procedure freeEvent(e: eventp);					external;
procedure sendCmd;						external;
procedure killNode(n: nodep);					external;

	(* From IAUX1B *)
procedure addPdb(var plist: pdbp; pn: pdbp);			external;
procedure sleep(whenV: integer);				external;

	(* From IAUX2B *)
procedure cmonEnable(e: enventryp);				external;

	(* From IROOT *)
function m2Forcebits(fn: nodep; var negv: boolean): integer;	external;
function m2GetMechbits: integer;				external;
procedure m2MvStart;						external;
procedure m2MvEnd;						external;
procedure m2MvRetry;						external;

	(* From RSXMSG *)
procedure signalArm;                                         	external;

	(* Display-related Routines *)
procedure ppLine; 						external;
procedure ppOutNow; 						external;
procedure ppChar(ch: ascii); 					external;
procedure pp5(ch: c5str; length: integer); 			external;
procedure pp10(ch: cstring; length: integer); 			external;
procedure pp10L(ch: cstring; length: integer);			external;
procedure pp20(ch: c20str; length: integer); 			external;
procedure pp20L(ch: c20str; length: integer); 			external;
procedure ppInt(i: integer); 					external;
procedure ppReal(r: real); 					external;
procedure ppStrng(length: integer; s: strngp); 			external;


procedure iMove2Get; external;
procedure iMove2Get; begin end;

procedure doCmon; external;
procedure doCmon;
var e: enventryp; n: nodep; b: boolean; val: nodep; r: real; fbits,i: integer;
    sst: statementp;
 begin
 with curInt↑ do
  case mode of
1: begin
   if not spc↑.deferCm then	(* check it's not a deferred cmon *)
     begin			(* need to enable the cmon *)
     with spc↑.cdef↑ do
      cmonEnable(getEntry(level,offset));	(* enable cmon control block *)
     end;
   mode := 0;
   spc := spc↑.next;
   end;

2: begin			(* deal with ON condition *)
   n := nil;
   mode := 3;			(* set up for doing conclusion next time *)
   if spc↑.exprCm then
     begin			(* test if expression is now true *)
     n := pop;			(* get expression value *)
     if n↑.s = 0.0 then
       begin
       sleep(20);		(* no good - try again in 0.33 seconds *)
       mode := 0;
       end;
     end
    else if spc↑.oncond↑.ntype = durnode then
     begin			(* duration cmon *)
     n := pop;
     sleep(round(n↑.s * 60));	(* get wait time (in 60Hz ticks) *)
     end
    else if spc↑.oncond↑.ntype = forcenode then
     begin					(* force sensing *)
     val := getNval(spc↑.oncond↑.fval,b);	(* get force magnitude *)
     r := val↑.s;
     if b then relNode(val);
     fbits := m2Forcebits(spc↑.oncond,b);
     with spc↑.oncond↑ do
      begin
      if (ftype = absforce) or (ftype = abstorque) then fbits := fbits + SIGMAG;
      if b then begin r := -r; if frel < seqop then fbits := fbits + SIGGE end
       else if frel >= seqop then fbits := fbits + SIGGE;
      end;
     with spc↑.conclusion↑ do
      if stype = stoptype then
	begin	(* set FSTOP bit if no explicit frame is being stopped *)
	if cf = nil then fbits := fbits + FSTOP
	 else if cf↑.ntype = leafnode then
	  begin		(* need to check if same device as current mech *)
	  e := gtVarn(cf);	(* get variable frame *)
	  if e↑.etype = frametype then
	    begin
	    if e↑.f = nil then i := GARMDEV		(* default to green arm *)
	     else with e↑.f↑ do
	      if ftype then
		if dev <> nil then i := dev↑.mech
		 else i := GARMDEV			(* default to green arm *)
	       else i := mech;
	    if i = m2GetMechBits then fbits := fbits + FSTOP;
	    end
	  end;
	(* ** can't check if array ref since subscripts aren't on stack ** *)
	end
       else if stype = blocktype then
	if bcode↑.stype = stoptype then
	  if bcode↑.cf = nil then fbits := fbits + FSTOP;
     cm↑.fbits := fbits;		(* remember bits in cmoncb *)
     with msg↑ do
      begin
      cmd := forcesigcmd;
      dev := m2GetMechbits;		(* deal with which arm here *)
      bits := fbits;
      evt := cm↑.evt;
      mag := r;
      end;
     sendCmd;
     cm↑.evt↑.count := -1;
     cm↑.evt↑.waitlist := curInt;	(* put us on event waitlist *)
     curInt↑.status := forcewait;
     curInt := nil;			(* swap in someone else *)
     resched := true;
     end
    else if spc↑.oncond↑.ntype = departingnode then
     begin			(* departing cmon *)
     sleep(30);			(* wait 0.5 seconds (in 60Hz ticks) *)
     end
    else
     begin			(* event cmon *)
     if spc↑.oncond↑.ntype = arrivalnode then
       with spc↑.oncond↑.evar↑ do e := getVar(level,offset)
      else e := gtVarn(spc↑.oncond);
     cm↑.evt := e↑.evt;		(* save pointer to event we're waiting on *)
     e↑.evt↑.count := e↑.evt↑.count - 1;
     if e↑.evt↑.count <= 0 then (* hasn't been signalled yet, need to wait *)
       begin
       addPdb(e↑.evt↑.waitlist,curInt);	(* add us to wait list *)
       curInt↑.status := eventqueue;
       curInt := nil;			(* swap in someone else *)
       resched := true;
       end;
     end;
   if n <> nil then relNode(n);
   end;

3: begin
   mode := 0;
   if cm↑.enabled then		(* check that we're still enabled *)
     begin
     cm↑.running := true;	(* set up current cmon status *)
     cm↑.enabled := false;
     spc := spc↑.conclusion;
     end
    else
     begin
     curInt↑.status := nullqueue;
     curInt := nil;		(* we should go away *)
     resched := true;		(* now swap in highest priority process *)
     end;
   end;

  end;
 end;

procedure doOperate; external;
procedure doOperate;
 var durcl,vel,torquecl,cl,v: nodep; e: enventryp; b,ccw: boolean; ev: eventp;
 begin				(* deal with driver *)
 with curInt↑ do
  case mode of
1:  begin
    e := gtVarn(spc↑.cf);	(* remember what we're moving *)
    mech := e↑.f;
    m2MvStart;			(* enable all condition monitors for move *)
    mode := 2;
    end;

2:  begin		(* set up motion specs for arm code & send it over *)
    ev := getEvent;	(* event to use for signalling when motion finishes *)
    ev↑.count := -1;
    ev↑.waitlist := curInt;

    durcl := nil;
    vel := nil;
    torquecl := nil;
    ccw := false;
    cl := spc↑.clauses;
    while cl <> nil do			(* run through clauses *)
     with cl↑ do
      begin
      if ntype = durnode then durcl := cl
       else if ntype = forcenode then
	begin
	if ftype = torque then torquecl := cl
	 else if ftype = angvelocity then vel := cl
	end
       else if ntype = cwnode then ccw := notp;
      cl := next;
      end;

    with msg↑ do
     begin
     cmd := operatecmd;
     dev := m2GetMechbits;
     bits := 0;
     evt := ev;
     dur := 5.0;		(* default values *)
     v1 := 60.0;		(* angular velocity *)
     v2 := 0.0;			(* torque *)

     if durcl <> nil then
       begin
       v := getNval(durcl↑.durval,b);		(* get duration value *)
       dur := v↑.s;
       if b then relNode(v);
       end;

     if vel <> nil then
       begin
       v := getNval(vel↑.fval,b);		(* get angular velocity value *)
       v1 := v↑.s;
       if b then relNode(v);
       end;

     if torquecl <> nil then
       begin
       v := getNval(torquecl↑.fval,b);		(* get torque value *)
       v2 := v↑.s;
       if b then relNode(v);
       end;

     if ccw then
       begin				(* turning counterclockwise *)
       v1 := - v1;
       v2 := - v2;
       end;
     end;

    sendCmd;				(* pass info to ARM servo *)
    mode := 3;
    curInt↑.status := devicewait; 	(* don't for simulation version *)
    curInt := nil;
    resched := true;			(* swap someone else in *)
    end;

3:  m2MvEnd;	(* do end of motion cleanup, run error handler, etc. *)

4:  m2MvRetry;	(* deal with user response if there was an error *)

  end;

 end;

procedure doOpen; (* & doClose *) external;
procedure doOpen; 
 var dest,bydest,sfac,durcl,swt,cl,v: nodep; e: enventryp; ev: eventp;
     opening,dtime,sf,swtime: real; mechbits: integer; b,nulling: boolean;
 begin
 with curInt↑ do
  case mode of
1:  begin
    e := gtVarn(spc↑.cf);	(* remember what we're moving *)
    mech := e↑.f;
    m2MvStart;		(* enable all condition monitors for move *)
    mode := 2;
    end;

2:  begin		(* set up motion specs for arm code & send it over *)
    ev := getEvent;	(* event to use for signalling when motion finishes *)
    ev↑.count := -1;
    ev↑.waitlist := curInt;
    mechbits := m2GetMechbits;

(* run through clauses for dest, duration & speed factor/stop wait time specs *)
    dest := nil;
    bydest := nil;
    durcl := nil;
    sfac := nil;
    swt := nil;
    nulling := true;			(* nonulling is the default *)
    cl := spc↑.clauses;
    while cl <> nil do			(* run through clauses *)
     with cl↑ do
      begin
      case ntype of
destnode:	dest := cl;
byptnode:	bydest := cl;
durnode:	durcl := cl;
sfacnode:	sfac := cl;
swtnode:	swt := cl;
nullingnode:	nulling := notp;
otherwise	begin (* nothing to do *) end;
	end;
      cl := next;
      end;

    if sfac = nil then sf := speedfactor↑.s	(* use global speed factor *)
     else
      begin
      v := getNval(sfac↑.clval,b);		(* get local speed factor value *)
      sf := v↑.s;
      if b then relNode(v);
      end;

    if durcl = nil then dtime := 0
     else
      begin
      v := getNval(durcl↑.durval,b);		(* get duration value *)
      dtime := v↑.s;
      if b then relNode(v);
      end;

    if swt = nil then swtime := 0
     else
      begin
      v := getNval(swt↑.clval,b);		(* get stop wait time value *)
      swtime := v↑.s;
      if b then relNode(v);
      end;

    if dest <> nil then
      begin
      v := getNval(dest↑.loc,b);		(* get opening value *)
      opening := v↑.s;
      mech↑.sdest := opening;			(* remember it *)
      if b then relNode(v);
      end
     else if bydest <> nil then
      begin
      v := getNval(bydest↑.loc,b);		(* get opening value *)
      opening := v↑.s;
      mech↑.sdest := mech↑.sdest + opening;	(* remember it *)
      if b then relNode(v);
      end
     else
      begin
      opening := 0;
      mech↑.sdest := -1;			(* so we know there was no dest *)
      end;

    with msg↑ do
     begin
     dev := mechbits;
     evt := ev;
     if nulling then bits := NULLINGCB else bits := 0;
     if (dest <> nil) or (bydest <> nil) then
       begin
       pos := opening;
       bits := bits + DESTPTCB;		(* indicate we're specifying opening *)
       if dest = nil then bits := bits + BYPTCB;  (* tell ARM incremental motion *)
       end
      else
       begin
       pos := 0.0;
       if spc↑.stype = opentype then bits := 3 else bits := 1;
       end;
     if durcl = nil then dur := 0.0
      else
       begin
       dur := dtime;
       bits := bits + DUREQCB;
       end;
     sfac := sf;

     if mechbits = VISEDEV then
       begin
       cmd := operatecmd;		(* vise uses an operate command *)
       if swt = nil then
	 if dest = nil then v2 := 0.25 else v2 := 0.0	(* default values *)
	else v2 := swtime;
       if durcl = nil then dur := 8.0;
       sendCmd;
       end
      else
       begin
       cmd := movehdrcmd;			(* deal with hand *)
       sendCmd;
       signalArm;	(* since movehdr normally followed by movesegs *)
       end;
     end;

    mode := 3;
    curInt↑.status := devicewait; 	(* don't for simulation version *)
    curInt := nil;
    resched := true;			(* swap someone else in *)
    end;

3:  m2MvEnd;	(* do end of motion cleanup, run error handler, etc. *)

4:  m2MvRetry;	(* deal with user response if there was an error *)

  end;

 end;

procedure doCenter; external;
procedure doCenter;
 var e: enventryp; ev: eventp;
 begin
 with curInt↑ do
  case mode of
1:  begin
    e := gtVarn(spc↑.cf);	(* remember what we're moving *)
    mech := e↑.f;
    m2MvStart;		(* enable all condition monitors for move *)
    mode := 2;
    end;

2:  begin		(* set up motion specs for arm code & send it over *)
    ev := getEvent;	(* event to use for signalling when motion finishes *)
    ev↑.count := -1;
    ev↑.waitlist := curInt;
    with msg↑ do
     begin
     cmd := centercmd;
     dev := m2GetMechbits;
     bits := 0;
     evt := ev;
     end;
    sendCmd;				(* initiate the center operation *)
    mode := 3;
    curInt↑.status := devicewait; 	(* don't for simulation version *)
    curInt := nil;
    resched := true;			(* swap someone else in *)
    end;

3:  m2MvEnd;	(* do end of motion cleanup, run error handler, etc. *)

4:  m2MvRetry;	(* deal with user response if there was an error *)

  end;

 end;

procedure doStop; external;
procedure doStop;
 var mechbits: integer; e: enventryp;

 procedure complain;
  begin		(* yow! frame that's not affixed to a device *)
  pp20L('Attempt to stop fram',20); pp20('e not affixed to any',20);
  pp20(' device: Assuming ga',20); pp5('rm   ',2); ppLine;
  mechbits := GARMDEV;
  end;

 begin
 with curInt↑ do
  begin
  if spc↑.cf = nil then
    if mech = nil then complain
     else mechbits := m2GetMechbits	(* use current mech *)
   else
    begin
    e := gtVarn(spc↑.cf);		(* see what we're stopping *)
    with e↑.f↑ do
     if ftype then
       if dev <> nil then mechbits := dev↑.mech
	else complain
      else mechbits := mech;
    end;
  with msg↑ do
   begin
   cmd := stopcmd;
   dev := mechbits;
   end;
  sendCmd;				(* tell arm servo to stop device *)
  mode := 0;
  spc := spc↑.next;
  end;
 end;

procedure doArmmagic; external;
procedure doArmmagic;
 var e: enventryp; ev: eventp; np: nodep; i,j,k: integer;
 begin
 with curInt↑ do
  case mode of
1:  begin
    np := pop;
    i := round(np↑.s);		(* get # of arm magic command *)
    relNode(np);
    e := gtVarn(spc↑.dev);	(* remember what we're moving *)
    mech := e↑.f;
    ev := getEvent;	(* event to use for signalling when motion finishes *)
    ev↑.count := -1;
    ev↑.waitlist := curInt;
    j := 0;
    np := spc↑.iargs;
    while np <> nil do begin np := np↑.next; j := j + 1 end;	(* count args *)
    with msg↑ do
     begin
     cmd := armmagiccmd;
     n := i;				(* command number *)
     dev := m2GetMechbits;
     bits := j;
     evt := ev;
     sendCmd;				(* initiate the armmagic operation *)
     for i := 1 to j do
      begin				(* send over the arguments *)
      np := pop;			(* get next argument *)
      if np↑.ltype = svaltype then
	begin
	cmd := realcmd;
	dur := np↑.s
	end
       else if np↑.ltype = vectype then
	begin
	cmd := vectorcmd;
	with np↑.v↑ do
	 begin
	 v1 := val[1];			(* copy vector *)
	 v2 := val[2];
	 v3 := val[3];
	 end
	end
       else if np↑.ltype = transtype then
	begin
	cmd := transcmd;
	with np↑.t↑ do
	 begin
	 for k := 1 to 3 do begin t[k] := val[k,1]; t[k+3] := val[k,2] end;
	 sendCmd;			(* send first packet of trans over *)
	 for k := 1 to 3 do begin t[k] := val[k,3]; t[k+3] := val[k,4] end;
	 end;
	end
       else
	begin			(* error -- must be string type *)
	pp20L('ARM MAGIC can''t hand',20); pp10('le strings',10); ppLine;
	cmd := realcmd;
	dur := 0.0;			(* send a zero instead *)
	end;
      sendCmd;			(* send real/vector/2nd-half-of-trans over *)
      killNode(np);			(* flush used stack entry *)
      end;
     end;
    signalArm;				(* start things happening *)

    mode := 2;
    status := devicewait;
    curInt := nil;
    resched := true;                  	(* swap someone else in *)
    end;

2:  begin
    mode := 0;				(* get ready for next statement *)
    spc := spc↑.next;
    end

   end;

end;